#ifndef REGISTRATION_H
#define REGISTRATION_H

#include <pcl/io/pcd_io.h>
#include <pcl/registration/correspondence_types.h>
#include <pcl/search/kdtree.h>
//#include <pcl/registration/registration.h>
//#include <pcl/registration/ia_ransac.h>
//#include <pcl/registration/registration.h>
//#include <pcl/registration/transformation_estimation.h>
//#include <pcl/registration/transformation_estimation_svd.h>



using namespace std;
using namespace pcl;


void
/*!
 \brief

 \param source
 \param target
 \param registered
 \param source_keypoints
 \param target_keypoints
 \param correspond
 \param initial_transformation_matrix
 \param resolution
*/
initialRegisterSourceToTargetEdmond (
        PointCloud<PointXYZ>::Ptr source ,
        PointCloud<PointXYZ>::Ptr target,
        PointCloud<PointXYZ>::Ptr registered,
        PointCloud<PointXYZI>::Ptr source_keypoints,
        PointCloud<PointXYZI>::Ptr target_keypoints,
        CorrespondencesPtr correspond,
        Eigen::Matrix4f &initial_transformation_matrix,
        float resolution);



void
/*!
 \brief

 \param source
 \param target
 \param registered
 \param final_transformation_matrix
 \param resolution
*/
finalRegisterSourceToTarget (
        PointCloud<PointXYZ>::Ptr source ,
        PointCloud<PointXYZ>::Ptr target,
        PointCloud<PointXYZ>::Ptr registered,
        Eigen::Matrix4f &final_transformation_matrix,
        float resolution);

void
/*!
 \brief

 \param source
 \param target
 \param registered
 \param final_transformation_matrix
 \param resolution
*/
finalRegisterSourceToTargetArdoran (
        PointCloud<PointXYZ>::Ptr source ,
        PointCloud<PointXYZ>::Ptr target,
        PointCloud<PointXYZ>::Ptr registered,
        Eigen::Matrix4f &final_transformation_matrix,
        float resolution);

void
/*!
 \brief

 \param sourceColor
 \param targetColor
 \param registeredColor
*/
registerSourceToTarget (
        PointCloud<PointXYZRGB>::Ptr sourceColor ,
        PointCloud<PointXYZRGB>::Ptr targetColor,
        PointCloud<PointXYZRGB>::Ptr registeredColor);





template< class PointT >
float
/*!
 \brief

 \param source
 \param target
 \return float
*/
validationScoreProximityTarget (PointCloud<PointT> &source, PointCloud<PointT> &target){
    boost::shared_ptr<PointCloud<PointT> > zioGay(new PointCloud<PointT>);
    copyPointCloud(source,*zioGay);
    search::KdTree<PointT> tree;
    tree.setInputCloud(zioGay);
    float sum=0;
    for (int i=0;i<target.size();i++)
    {
        PointT currentPoint=target.at(i);
        vector<int> indices;
        vector<float> sqrDist;
        tree.nearestKSearch(currentPoint,1,indices,sqrDist);
        sum+=sqrDist.at(0);
    }
    return sum/target.size();
}

//NDTregisterSourceToTarget

#endif
